﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using ManagedNite;
using System.Windows;
using System.Windows.Media;
using Kinect.Framework;
using Kinect.Framework.Controls;
using Kinect.Framework.Interfaces;
using Kinect.Framework.Lib;
using System.ComponentModel;
using System.Windows.Threading;
using System.Threading;

namespace Kinect.Framework.Controller
{
    public class KinectManipulationController
    {
        private XnMSessionManager sessionManager;
        private XnMOpenNIContext context;
        public double ActualWidth { get; set; }
        public double ActualHeight { get; set; }
        public List<KinectPointer> Pointers { get; set; }
        public bool IsScaleEnabled { get; protected set; }
        private XnMSlider3D Slider3D;
        public bool ForceOneHandNavigation { get; set; }
        private double SwipeLantecy = 800;
        private bool IsSwipeDetected = false;
        private float DenoiserDistanceThresold = 2.5f;

        public enum ScrollOrientation { UP, DOWN, LEFT, RIGHT }

        #region Events SwipeManipulationDetected
        public delegate void SwipeManipulationDetectedEventHandler(object s, PayloadEventArgs<KinectSwipeManipulation> e);
        public event SwipeManipulationDetectedEventHandler SwipeManipulationDetected;
        public void OnSwipeManipulationDetected(KinectSwipeManipulation value)
        {
            if (SwipeManipulationDetected != null)
                SwipeManipulationDetected(this, new PayloadEventArgs<KinectSwipeManipulation>(value));
        }
        #endregion

        #region Events PushManipulationDetected
        public delegate void PushManipulationDetectedEventHandler(object s, PayloadEventArgs<KinectManipulationBase> e);
        public event PushManipulationDetectedEventHandler PushManipulationDetected;
        public void OnPushManipulationDetected(KinectManipulationBase value)
        {
            if (PushManipulationDetected != null)
                PushManipulationDetected(this, new PayloadEventArgs<KinectManipulationBase>(value));
        }
        #endregion

        #region Events WaveManipulationDetected
        public delegate void WaveManipulationDetectedEventHandler(object s, EventArgs e);
        public event WaveManipulationDetectedEventHandler WaveManipulationDetected;
        public void OnWaveManipulationDetected()
        {
            if (WaveManipulationDetected != null)
                WaveManipulationDetected(this, new EventArgs());
        }
        #endregion

        #region Events SteadyManipulationDetected
        public delegate void SteadyManipulationDetectedEventHandler(object s, PayloadEventArgs<KinectPointer> e);
        public event SteadyManipulationDetectedEventHandler SteadyManipulationDetected;
        public void OnSteadyManipulationDetected(KinectPointer value)
        {
            if (SteadyManipulationDetected != null)
                SteadyManipulationDetected(this, new PayloadEventArgs<KinectPointer>(value));
        }
        #endregion

        #region Events ManipulationDetected
        public delegate void ManipulationDetectedEventHandler(object s, PayloadEventArgs<KinectManipulationDelta> e);
        public event ManipulationDetectedEventHandler ManipulationDetected;
        public void OnManipulationDetected(KinectManipulationDelta value)
        {
            if (ManipulationDetected != null)
                ManipulationDetected(this, new PayloadEventArgs<KinectManipulationDelta>(value));
        }
        #endregion

        #region Events KinectPointerCreated
        public delegate void KinectPointerCreatedEventHandler(object s, PayloadEventArgs<KinectPointer> e);
        public event KinectPointerCreatedEventHandler KinectPointerCreated;
        public void OnKinectPointerCreated(KinectPointer value)
        {
            if (KinectPointerCreated != null)
                KinectPointerCreated(this, new PayloadEventArgs<KinectPointer>(value));
        }
        #endregion

        #region Events KinectPointerDestroyed
        public delegate void KinectPointerDestroyedEventHandler(object s, PayloadEventArgs<KinectPointer> e);
        public event KinectPointerDestroyedEventHandler KinectPointerDestroyed;
        public void OnKinectPointerDestroyed(KinectPointer value)
        {
            if (KinectPointerDestroyed != null)
                KinectPointerDestroyed(this, new PayloadEventArgs<KinectPointer>(value));
        }
        #endregion

        #region Events FocusDetected
        public delegate void FocusDetectedEventHandler(object s, EventArgs e);
        public event FocusDetectedEventHandler FocusDetected;
        public void OnFocusDetected()
        {
            if (FocusDetected != null)
                FocusDetected(this, new EventArgs());
        }
        #endregion

        #region Events TrackingStarted
        public delegate void TrackingStartedEventHandler(object s, EventArgs e);
        public event TrackingStartedEventHandler TrackingStarted;
        public void OnTrackingStarted()
        {
            if (TrackingStarted != null)
                TrackingStarted(this, new EventArgs());
        }
        #endregion

        #region Events Stopped
        public delegate void StoppedEventHandler(object s, EventArgs e);
        public event StoppedEventHandler Stopped;
        public void OnStopped()
        {
            if (Stopped != null)
                Stopped(this, new EventArgs());
        }
        #endregion

        #region Events ScaleEnabled
        public delegate void ScaleEnabledEventHandler(object s, EventArgs e);
        public event ScaleEnabledEventHandler ScaleEnabled;
        public void OnScaleEnabled()
        {
            //Console.WriteLine("OnScaleEnabled");
            if (ScaleEnabled != null)
                ScaleEnabled(this, new EventArgs());
        }
        #endregion

        #region Events ScaleDisabled
        public delegate void ScaleDisabledEventHandler(object s, EventArgs e);
        public event ScaleDisabledEventHandler ScaleDisabled;
        public void OnScaleDisabled()
        {
            //Console.WriteLine("OnScaleDisabled");
            if (ScaleDisabled != null)
                ScaleDisabled(this, new EventArgs());
        }
        #endregion

        #region Events ScaleCreated
        public delegate void ScaleCreatedEventHandler(object s, EventArgs e);
        public event ScaleCreatedEventHandler ScaleCreated;
        public void OnScaleCreated()
        {
            //Console.WriteLine("OnScaleCreated");
            if (ScaleCreated != null)
                ScaleCreated(this, new EventArgs());
        }
        #endregion

        #region Events ScaleDestroyed
        public delegate void ScaleDestroyedEventHandler(object s, EventArgs e);
        public event ScaleDestroyedEventHandler ScaleDestroyed;
        public void OnScaleDestroyed()
        {
            //Console.WriteLine("OnScaleDestroyed");
            if (ScaleDestroyed != null)
                ScaleDestroyed(this, new EventArgs());
        }
        #endregion

        #region Events ScaleUpdated
        public delegate void ScaleUpdatedEventHandler(object s, PayloadEventArgs<double> e);
        public event ScaleUpdatedEventHandler ScaleUpdated;
        public void OnScaleUpdated(double value)
        {
            //Console.WriteLine("OnOnScaleUpdated");
            if (ScaleUpdated != null)
                ScaleUpdated(this, new PayloadEventArgs<double>(value));
        }
        #endregion

        #region Events TwoHandScrolling
        public delegate void TwoHandScrollingEventHandler(object s, PayloadEventArgs<ScrollOrientation> e);
        public event TwoHandScrollingEventHandler TwoHandScrolling;
        public void OnTwoHandScrolling(ScrollOrientation value)
        {
            //Console.WriteLine("OnTwoHandScrolling");
            if (TwoHandScrolling != null)
                TwoHandScrolling(this, new PayloadEventArgs<ScrollOrientation>(value));
        }
        #endregion

        public bool HasPointerSelectedElement { get; set; }

        public void Start()
        {
            context = new XnMOpenNIContext();
            context.Init();

            sessionManager = new XnMSessionManager(context, "Wave,Click", "RaiseHand");
            sessionManager.QuickRefocusTimeout = 1000;
            sessionManager.FocusStartDetected += new EventHandler<FocusStartEventArgs>(OnFocusStartDetected);
            sessionManager.SessionStarted += new EventHandler<PointEventArgs>(OnSessionStarted);
            sessionManager.SessionEnded += new EventHandler(OnSessionEnded);

            XnMPointDenoiser denoiser = new XnMPointDenoiser(50);

            denoiser.PointCreate += new EventHandler<PointBasedEventArgs>(OnPointCreate);
            denoiser.PointDestroy += new EventHandler<PointDestroyEventArgs>(OnPointDestroy);
            denoiser.PointUpdate += new EventHandler<PointBasedEventArgs>(OnPointUpdate);
            XnMWaveDetector wave = new XnMWaveDetector();
            wave.Wave += new EventHandler(OnWave);

            XnMPushDetector push = new XnMPushDetector();
            push.Push += new EventHandler<PushDetectorEventArgs>(OnPush);

            XnMSteadyDetector steady = new XnMSteadyDetector();
            steady.DetectionDuration = 2000;
            steady.Steady += new EventHandler<SteadyEventArgs>(OnSteady);

            sessionManager.AddListener(denoiser);
            sessionManager.AddListener(steady);
            sessionManager.AddListener(wave);
            sessionManager.AddListener(push);

            Thread th = new Thread(new ThreadStart(SpinInfinite));
            th.Priority = ThreadPriority.Lowest;
            th.Start();



        }

        public void SetDenoiserDistanceThresold(float Distance)
        {
            DenoiserDistanceThresold = Distance;
        }
        void OnSteady(object sender, SteadyEventArgs e)
        {
            Application.Current.Dispatcher.Invoke(DispatcherPriority.Background, new Action(() =>
            {
                if (!HasPointerSelectedElement)
                {
                    var pointer = Pointers.Where(p => p.Id == Convert.ToInt32(((XnMSteadyDetector)sender).PrimaryID)).FirstOrDefault();
                    OnSteadyManipulationDetected(pointer);
                }
            }));
        }

        private void SpinInfinite()
        {
            while (true)
            {
                uint rc = context.Update();
                if (rc == 0)
                    sessionManager.Update(context);
            }
        }

        public void Stop()
        {
            context.Close();
        }

        void OnPush(object sender, PushDetectorEventArgs e)
        {
            Application.Current.Dispatcher.Invoke(DispatcherPriority.Background, new Action(() =>
            {
                var manipulation = new KinectManipulationBase()
                {
                    Angle = e.Angle,
                    Velocity = e.Velocity
                };
                OnPushManipulationDetected(manipulation);
            }));
        }



        void OnWave(object sender, EventArgs e)
        {
            Application.Current.Dispatcher.Invoke(DispatcherPriority.Background, new Action(() =>
            {
                OnWaveManipulationDetected();
            }));
        }
        public void CreateSlider3D(XnMPoint min, XnMPoint max)
        {
            if (Slider3D == null)
            {
                Slider3D = new XnMSlider3D(min, max);
                //Console.WriteLine("min :" + min.X + "/" + min.Y + "/" + min.Z);
                //Console.WriteLine("max :" + max.X + "/" + max.Y + "/" + max.Z);
                Slider3D.ValueChange -= new EventHandler<Slider3DValueChangeEventArgs>(OnSlider3DValueChange);
                Slider3D.ValueChange += new EventHandler<Slider3DValueChangeEventArgs>(OnSlider3DValueChange);
                OnScaleCreated();
            }
        }

        void OnSlider3DValueChange(object sender, Slider3DValueChangeEventArgs e)
        {
            int ActualWidth = Convert.ToInt32(this.ActualWidth);
            int ActualHeight = Convert.ToInt32(this.ActualHeight);

            int width = 640;
            int height = 480;

            double ratioWidth = ActualWidth / width;
            double ratioHeight = ActualHeight / height;

            if (e.X != 0)
            {
                OnScaleUpdated(Convert.ToDouble(e.X * ratioWidth));
            }
        }
        void DetectTwoHandScroll(KinectPointer first, KinectPointer second)
        {
            Application.Current.Dispatcher.Invoke(DispatcherPriority.Background, new Action(() =>
            {
                if (first.Position == null || second.Position == null)
                    return;

                var stepVertical = Math.Floor(this.ActualHeight / 10);
                var zoneTopTo = stepVertical * 2;
                var zoneBottomFrom = stepVertical * 8;
                if (first.Position.Y < zoneTopTo && second.Position.Y < zoneTopTo)
                    OnTwoHandScrolling(ScrollOrientation.UP);
                if (zoneBottomFrom < first.Position.Y && zoneBottomFrom < second.Position.Y)
                    OnTwoHandScrolling(ScrollOrientation.DOWN);

                var stepHorizontal = Math.Floor(this.ActualWidth / 10);
                var zoneLeft = stepHorizontal * 2;
                var zoneRight = stepHorizontal * 8;
                if (first.Position.X < zoneLeft && second.Position.X < zoneLeft)
                    OnTwoHandScrolling(ScrollOrientation.LEFT);
                if (first.Position.X > zoneRight && second.Position.X > zoneRight)
                    OnTwoHandScrolling(ScrollOrientation.RIGHT);
            }));
        }

        XnMPoint ConvertPointToRealWorld(XnMPoint e)
        {
            int width = 640;
            int height = 480;

            double dPosX = ((width / 2) - e.X) * GetRatioWidth();
            double dPosY = ((height / 2) - e.Y) * GetRatioHeight();
            double dPosZ = e.Z;

            int posX = Convert.ToInt32(dPosX);
            int posY = Convert.ToInt32(dPosY);
            int posZ = Convert.ToInt32(dPosZ);

            return new XnMPoint()
            {
                X = posX,
                Y = posY,
                Z = posZ
            };
        }

        double GetRatioHeight()
        {
            int height = 480;
            double ratioHeight = ActualHeight / height;
            return ratioHeight;
        }

        double GetRatioWidth()
        {
            int width = 640;
            double ratioWidth = ActualWidth / width;
            return ratioWidth;
        }

        XnMPoint GetDeltaPosition(XnMPoint lastPoint, XnMPoint newPoint)
        {
            return new XnMPoint()
            {
                X = Math.Abs(lastPoint.X - newPoint.X),
                Y = Math.Abs(lastPoint.Y - newPoint.Y),
                Z = Math.Abs(lastPoint.Z - newPoint.Z),
            };
        }


        void OnPointUpdate(object sender, PointBasedEventArgs e)
        {
            Application.Current.Dispatcher.Invoke(DispatcherPriority.Background, new Action(() =>
            {
                var pointer = Pointers.Where(p => p.Id == e.Id).FirstOrDefault();
                if (pointer == null)
                    return;
                if (Pointers.Count == 2 && !ForceOneHandNavigation)
                {
                    DetectTwoHandScroll(Pointers[0], Pointers[1]);
                }

                if (pointer.RealPosition != null
                    && Math.Abs(e.Position.X - pointer.RealPosition.X) < DenoiserDistanceThresold
                    && Math.Abs(e.Position.Y - pointer.RealPosition.Y) < DenoiserDistanceThresold)
                    return;

                var updatePoint = e.Position;
                var newPoint = ConvertPointToRealWorld(e.Position);

                double angle = 0.0;
                double vector = 0.0;

                var manipulationDelta = new KinectManipulationDelta();

                if (Slider3D != null && !ForceOneHandNavigation)
                    Slider3D.Update(updatePoint);


                if (pointer.LastPoint != null)
                {
                    var lastPoint = ConvertPointToRealWorld(pointer.LastPoint);
                    var deltaPoint = GetDeltaPosition(pointer.LastPoint, updatePoint);

                    vector = Math.Sqrt(Math.Pow(deltaPoint.X, 2) + Math.Pow(deltaPoint.Y, 2));
                    angle = Math.Acos(deltaPoint.X / vector) * 180.0 / Math.PI;

                    manipulationDelta = new KinectManipulationDelta()
                    {
                        Id = e.Id,
                        Distance = vector,
                        Angle = angle,
                        LastPosition = lastPoint,
                        Position = newPoint,
                        RealPosition = updatePoint,

                    };
                    OnManipulationDetected(manipulationDelta);

                    if (angle < 30
                        && vector > 100
                        )
                    {
                        if (!IsSwipeDetected)
                        {
                            IsSwipeDetected = true;
                            DispatcherTimer dt = new DispatcherTimer();
                            dt.Tick += (se, ev) =>
                            {
                                dt.Stop();
                                IsSwipeDetected = false;
                            };
                            dt.Interval = TimeSpan.FromMilliseconds(SwipeLantecy);
                            dt.Start();
                            KinectSwipeManipulation swipeManipulation = new KinectSwipeManipulation()
                            {
                                Angle = float.Parse(angle.ToString())
                            };
                            if (lastPoint.X < newPoint.X)
                                swipeManipulation.Type = SwipeType.LeftToRight;
                            else
                                swipeManipulation.Type = SwipeType.RightToLeft;

                            OnSwipeManipulationDetected(swipeManipulation);
                            pointer.LastPoint = updatePoint;
                            return;
                        }

                    }
                }
                else
                {
                    manipulationDelta = new KinectManipulationDelta()
                        {
                            Id = e.Id,
                            Distance = vector,
                            Angle = angle,
                            LastPosition = null,
                            Position = newPoint,
                            RealPosition = updatePoint,
                        };
                    OnManipulationDetected(manipulationDelta);
                }

                pointer.Move(manipulationDelta.Position, manipulationDelta.RealPosition);
                pointer.LastPoint = updatePoint;
            }));
        }


        void OnPointDestroy(object sender, PointDestroyEventArgs e)
        {
            Application.Current.Dispatcher.Invoke(DispatcherPriority.Background, new Action(() =>
            {
                var pointer = Pointers.Where(p => p.Id == e.ID).FirstOrDefault();
                OnKinectPointerDestroyed(pointer);
                Pointers.Remove(pointer);
                if (Pointers.Count != 2 && IsScaleEnabled && Slider3D != null && !ForceOneHandNavigation)
                {
                    Slider3D = null;
                    IsScaleEnabled = false;
                    OnScaleDestroyed();
                }
            }));
        }

        void OnPointCreate(object sender, PointBasedEventArgs e)
        {
            Application.Current.Dispatcher.Invoke(DispatcherPriority.Background, new Action(() =>
            {

                if (Pointers == null)
                    Pointers = new List<KinectPointer>();
                if (Pointers.Count == 1 && ForceOneHandNavigation)
                    return;
                var kinectPointer = new KinectPointer() { Id = Convert.ToUInt32(e.Id) };
                Pointers.Add(kinectPointer);
                OnKinectPointerCreated(kinectPointer);
                if (Pointers.Count == 2 && !ForceOneHandNavigation)
                    IsScaleEnabled = true;
            }));
        }

        void OnSessionEnded(object sender, EventArgs e)
        {
            OnStopped();
        }

        void OnSessionStarted(object sender, PointEventArgs e)
        {
            OnTrackingStarted();
            sessionManager.TrackPoint(e.Point);

        }

        void OnFocusStartDetected(object sender, FocusStartEventArgs e)
        {
            OnFocusDetected();
        }
    }
}
